package mskruch.gps.util;


import gpstools.Location;
import gpstools.TrainingPoint;
import gpstools.track.TrackPoint;

import java.util.ArrayList;
import java.util.Arrays;
import java.util.LinkedList;
import java.util.List;

import mskruch.gps.Point;
import mskruch.gps.PointFactory;
import mskruch.gps.Position;
import mskruch.gps.calculation.Calculations;
import mskruch.gps.calculation.PositionsCalculator;

public class PointsUtil {

    public static Point calculatePointByDistance(final Point[] points, final double distance) {
        double fromStart = 0;
        for (int i = 1; i < points.length; i++) {
            final Point prev = points[i - 1];
            final Point curr = points[i];
            final double d = curr.distanceTo(prev);
            fromStart += d;
            if (fromStart >= distance) {
                final double rev = fromStart - distance;
                final double ratio = rev / d;

                long time = Math.round(curr.getTime() - (curr.getTime() - prev.getTime()) * ratio);
                double lon = curr.getLongitude() - (curr.getLongitude() - prev.getLongitude()) * ratio;
                double lat = curr.getLatitude() - (curr.getLatitude() - prev.getLatitude()) * ratio;
                double ele = curr.getElevation() - (curr.getElevation() - prev.getElevation()) * ratio;
                Short hr = null;
                if (curr.getHeartRate() != null && prev.getHeartRate() != null) {
                    hr = (short) Math.round(curr.getHeartRate() - (curr.getHeartRate() - prev.getHeartRate()) * ratio);
                }
                Short cad = null;
                if (curr.getCadence() != null && prev.getCadence() != null) {
                    hr = (short) Math.round(curr.getCadence() - (curr.getCadence() - prev.getCadence()) * ratio);
                }
                return PointFactory.newPoint(lon, lat, ele, time, hr, cad);
            }
        }
        return null;
    }

    public static Position lineSegmentCenter(Position segmentStart, Position segmentEnd) {
        double lon = (segmentStart.getLongitude() + segmentEnd.getLongitude()) / 2;
        double lat = (segmentStart.getLatitude() + segmentEnd.getLatitude()) / 2;
        return PointFactory.newPosition(lon, lat);
    }

    public static Position nearestSegmentPosition(Position segmentStart, Position segmentEnd, Position point,
            double delta) {
        Position a = segmentStart;
        Position b = segmentEnd;
        Position c = a; /* result */

        double aDistance = a.distanceTo(point);
        double bDistance = b.distanceTo(point);
        while (a.distanceTo(b) > delta) {
            if (aDistance < bDistance) {
                c = a;
                b = lineSegmentCenter(a, b);
                bDistance = b.distanceTo(point);
            } else {
                c = b;
                a = lineSegmentCenter(a, b);
                aDistance = a.distanceTo(point);
            }
        }
        return c;
    }

    public static Point nearestPolygonPoint(Point[] polygon, Position point) {
        if (polygon == null || polygon.length < 2) {
            throw new IllegalArgumentException("polygon must have at least 2 points");
        }
        Double distance = null;
        Point nearest = null;
        for (int i = 0; i < polygon.length - 1; i++) {
            Point next = nearestSegmentPoint(polygon[i], polygon[i + 1], point);
            double distanceToPoint = next.distanceTo(point);
            if (distance == null || distanceToPoint < distance) {
                nearest = next;
                distance = distanceToPoint;
            }
        }
        return nearest;
    }
    
    public static Position nearestPolygonPosition(Position[] polygon, Position point) {
        if (polygon == null || polygon.length < 2) {
            throw new IllegalArgumentException("polygon must have at least 2 points");
        }
        Double distance = null;
        Position nearest = null;
        for (int i = 0; i < polygon.length - 1; i++) {
            Position next = nearestSegmentPosition(polygon[i], polygon[i + 1], point);
            double distanceToPoint = next.distanceTo(point);
            if (distance == null || distanceToPoint < distance) {
                nearest = next;
                distance = distanceToPoint;
            }
        }
        return nearest;
    }

    public static Point nearestSegmentPoint(Point segmentStart, Point segmentEnd, Position point) {
        return nearestSegmentPoint(segmentStart, segmentEnd, point, 0.01);
    }
    
    public static Position nearestSegmentPosition(Position segmentStart, Position segmentEnd, Position point) {
        return  nearestSegmentPosition(segmentStart, segmentEnd, point, 0.01);
    }

    public static Point nearestSegmentPoint(Point segmentStart, Point segmentEnd, Position point, double delta) {
        if (segmentStart.distanceTo(segmentEnd) == 0) {
            return segmentStart;
        }
        Position nearest = nearestSegmentPosition(segmentStart, segmentEnd, point, delta);

        double ratio = segmentStart.distanceTo(nearest) / segmentStart.distanceTo(segmentEnd);

        return createInterpolatedPoint(segmentStart, segmentEnd, ratio);
    }

    public static Position appNearestSegmentPosition(Position segmentStart, Position segmentEnd, Position point) {
        double x1 = segmentStart.getLongitude();
        double y1 = segmentStart.getLatitude();
        double x2 = segmentEnd.getLongitude();
        double y2 = segmentEnd.getLatitude();

        /* równanie na prostą przez dwa punkty */
        double A = y1 - y2;
        double B = x2 - x1;
        double C = x1 * (y2 - y1) - y1 * (x2 - x1);

        double xp = point.getLongitude();
        double yp = point.getLatitude();

        /* prostopadła do prostej przechodząca przez punkt */
        double A2 = -B;
        double B2 = A;
        double C2 = -A2 * xp - B2 * yp;

        /*
         * punkt przecięcia prostych - najbliższy punkt do podanego, znajdujący
         * się na odcinku
         */
        double dx = (B * C2 - C * B2) / (A * B2 - B * A2);
        double dy = (A * C2 - C * A2) / (B * A2 - A * B2);

        if (between(x1, x2, dx) && between(y1, y2, dy)) {
            /* punkt przecięcia należy do odcinka */
            // TODO: zweryfikowac czy mozna bezpiecznie usunac min ponizej
            // result = Math.min(DistanceUtil.distance(dy, dx, yp, xp), result);
            return PointFactory.newPosition(dx, dy);
        } else {
            /* jeżeli nie należy to zwracam odległość od krawędzi */
            if (segmentStart.distanceTo(point) < segmentEnd.distanceTo(point)) {
                return segmentStart;
            } else {
                return segmentEnd;
            }
        }
    }

    public static double appDistanceToLineSegment(Position segmentStart, Position segmentEnd, Position point) {
        return appNearestSegmentPosition(segmentStart, segmentEnd, point).distanceTo(point);
    }

    public static boolean between(double a, double b, double x) {
        if (a == b) {
            return x == a;
        } else if (a < b) {
            return x >= a && x <= b;
        } else { /* b < a */
            return x >= b && x <= a;
        }
    }

    @Deprecated
    public static Point[] findCheckPoints(Point[] points, int checks) {
        PositionsCalculator pc = Calculations.newPositionsCalculator();
        double totalDistance = pc.add(points).getDistance();
        Point[] checkPoints = new Point[checks];
        double[] checkDistances = new double[checks];
        for (int i = 0; i < checkDistances.length; i++) {
            checkDistances[i] = ((double) (i + 1)) * totalDistance / (checks + 1);
        }

        int index = 0; /* checkDistances */
        double dist = 0;
        for (int i = 1; i < points.length; i++) {
            final Point prev = points[i - 1];
            final Point curr = points[i];

            dist += curr.distanceTo(prev);
            if (index < checks && dist >= checkDistances[index]) {
                checkPoints[index] = curr;
                index++;
            }
        }
        return checkPoints;
    }

    public static Point[] copyOfRangeByDistance(Point[] points, double begin, double end) {
        int first = 0;
        int afterLast = points.length;
        boolean found = false;
        PositionsCalculator pc = Calculations.newPositionsCalculator();
        for (int i = 0; i < points.length; i++) {
            pc.add(points[i]);
            if (!found && pc.getDistance() >= begin) {
                first = i;
                found = true;
            } else if (found && pc.getDistance() > end) {
                afterLast = i;
                break;
            }
        }
        return Arrays.copyOfRange(points, first, afterLast);
    }

    public static double[] distanceInTime(TrainingPoint[] points, long interval, double ratio) {
        Point[] points2 = new Point[points.length];
        for (int i = 0; i < points.length; i++) {
            points2[i] = PointFactory.newPoint(points[i]);
        }
        return distanceInTime(points2, interval, ratio);
    }
    
    public static double[] distanceInTime(Point[] points, long interval, double ratio) {
        PositionsCalculator pc = Calculations.newPositionsCalculator();
        List<Double> distances = new ArrayList<Double>();
        long current = 0;
        long start = points[0].getTime();

        int i = 0;
        while (i < points.length) {
            while (i < points.length && points[i].getTime() - start <= current) {
                pc.add(points[i++]);
            }
            double currentDistance = pc.getDistance();
            if (i < points.length && i > 0) {
                double timeToLast = current - (points[i - 1].getTime() - start);
                double timeBetween = points[i].getTime() - points[i - 1].getTime();
                currentDistance += points[i - 1].distanceTo(points[i]) * timeToLast / timeBetween;
            }
            distances.add(currentDistance * ratio);
            current += interval;
        }
        double[] result = new double[distances.size()];
        for (int j = 0; j < result.length; j++) {
            result[j] = distances.get(j);
        }
        return result;
    }

    public static Position createInterpolatedPosiotion(Position segmentStart, Position segmentEnd, double ratio) {
        double lon = (segmentEnd.getLongitude() - segmentStart.getLongitude()) * ratio + segmentStart.getLongitude();
        double lat = (segmentEnd.getLatitude() - segmentStart.getLatitude()) * ratio + segmentStart.getLatitude();
        return PointFactory.newPosition(lon, lat);
    }

    public static Location createInterpolatedPosition(Location segmentStart, Location segmentEnd, double ratio) {
        double lon = (segmentEnd.getLongitude() - segmentStart.getLongitude()) * ratio + segmentStart.getLongitude();
        double lat = (segmentEnd.getLatitude() - segmentStart.getLatitude()) * ratio + segmentStart.getLatitude();
        return new gpstools.Point(lon, lat);
    }
    
    public static Point createInterpolatedPoint(Point segmentStart, Point segmentEnd, double ratio) {
        long time = (long) (segmentStart.getTime() + ratio * (segmentEnd.getTime() - segmentStart.getTime()));
        double elevation = segmentStart.getElevation() + ratio
                * (segmentEnd.getElevation() - segmentStart.getElevation());

        Short hr = segmentStart.getHeartRate();
        Short cad = segmentStart.getCadence();
        Position createInterpolatedPosiotion = createInterpolatedPosiotion(segmentStart, segmentEnd, ratio);
        return PointFactory.newPoint(createInterpolatedPosiotion.getLongitude(), createInterpolatedPosiotion
                .getLatitude(), elevation, time, hr, cad);
    }

    public static TrainingPoint createInterpolatedPoint(TrainingPoint segmentStart, TrainingPoint segmentEnd, double ratio) {
        long time = (long) (segmentStart.getTime() + ratio * (segmentEnd.getTime() - segmentStart.getTime()));
        double elevation = segmentStart.getElevation() + ratio
                * (segmentEnd.getElevation() - segmentStart.getElevation());
        
        Short hr = segmentStart.getHeartRate();
        Short cad = segmentStart.getCadence();
        Location location = createInterpolatedPosition(segmentStart, segmentEnd, ratio);
        
        return TrackPoint.builder(new gpstools.Point(location.getLatitude(), location.getLongitude(), elevation)).time(time).heartRate(hr).cadence(cad).build();
    }

    public static Point[] pointsInDistance(Point[] points, double distanceInterval, double ratio) {

        List<Point> resultList = new LinkedList<Point>();
        PositionsCalculator pc = Calculations.newPositionsCalculator();
        double previousDistance = 0.0;
        Point previousSourcePoint = points[0];

        pc.add(previousSourcePoint);
        int currentStep = 1;
        for (int i = 1; i < points.length; i++) {
            Point currentSourcePoint = points[i];
            pc.add(currentSourcePoint);
            double currentDistance = ratio * pc.getDistance();

            while (currentStep * distanceInterval <= currentDistance) {

                double localRatio = (currentStep * distanceInterval - previousDistance)
                        / (currentDistance - previousDistance);

                resultList.add(createInterpolatedPoint(previousSourcePoint, currentSourcePoint, localRatio));
                currentStep++;
            }

            previousDistance = currentDistance;
            previousSourcePoint = currentSourcePoint;
        }

        return resultList.toArray(new Point[resultList.size()]);
    }

    public static TrainingPoint[] pointsInDistance(TrainingPoint[] points, double distanceInterval, double ratio) {
        
        List<TrainingPoint> resultList = new LinkedList<TrainingPoint>();
        PositionsCalculator pc = Calculations.newPositionsCalculator();
        double previousDistance = 0.0;
        TrainingPoint previousSourcePoint = points[0];
        
        pc.add(previousSourcePoint);
        int currentStep = 1;
        for (int i = 1; i < points.length; i++) {
            TrainingPoint currentSourcePoint = points[i];
            pc.add(currentSourcePoint);
            double currentDistance = ratio * pc.getDistance();
            
            while (currentStep * distanceInterval <= currentDistance) {
                
                double localRatio = (currentStep * distanceInterval - previousDistance)
                        / (currentDistance - previousDistance);
                
                resultList.add(createInterpolatedPoint(previousSourcePoint, currentSourcePoint, localRatio));
                currentStep++;
            }
            
            previousDistance = currentDistance;
            previousSourcePoint = currentSourcePoint;
        }
        
        return resultList.toArray(new TrainingPoint[resultList.size()]);
    }

}
